classdef newSingleJoint <handle
      %默认添加重力，不要应该手动disable
    properties
        m   = 0.5;         % mass of robot
        n_factor;         %用于不同的n
        Ir   = 1.19e-5;        % MOI of motor ,I1自己算
        I1 = 0;
        l   = 0.1;         % length of robot
        Im;   %等效Im
        g   = 9.81;        % Gravity Constant
        B1  = 2e-2;         % 阻尼系数
        Br  = 4.1e-4; 
        Bm;   %等效阻尼系数
        
        %以顺时针为正方向
        is_g_enable;    %0 for no applying
        state;  % pos& vel & acc,三维，是减速前的，行，可以有     多行，表示不同组的输入       
        n_group = 1;
        
        joint_state;
       
    end
    
    methods
        function obj = newSingleJoint(origin_state,is_g_enable,n_factor,m,I1,l,B1,Br)
            obj.n_group = size(origin_state,1);    
            if nargin == 8
                obj.m = m;
                obj.n_factor = n_factor;
                obj.I1 = I1;
                obj.l = l;
                obj.B1 = B1;
                obj.Br = Br;
                obj.is_g_enable = is_g_enable;
                obj.state = origin_state;  
                obj.joint_state = origin_state;
            elseif nargin == 1
                obj.state = origin_state;
                obj.joint_state = origin_state;
                obj.is_g_enable = ones(obj.n_group,obj.n_group);
                obj.n_factor = repmat(10,obj.n_group,1);
                disp("only update origin state,g is enabled");
            elseif nargin == 2
                obj.state = origin_state;
                obj.joint_state = origin_state;
                obj.is_g_enable = is_g_enable;
                obj.n_factor = repmat(10,obj.n_group,1);
            elseif nargin == 3
                obj.state = origin_state;
                obj.joint_state = origin_state;
                obj.is_g_enable = is_g_enable;
                obj.n_factor = n_factor;
            else
                ME = MException('need at least 1 param for initial state');
                throw(ME)
            end


            obj.I1 = (obj.m*obj.l^2);
            obj.Im = obj.Ir;
            obj.Bm = obj.Br;
            %之后选择在仿真那里补上I1，B1
       
            
        end
        
        function reset_state(obj,origin_state)
            obj.state = zeros(obj.n_group,3);
            obj.joint_state = origin_state;
        end

        
        function state_info = simu_loop(obj,input_tau,simu_period)          
            %这里的state-info是三维的
            %                 obj.I1 = (obj.m*obj.l^2);
%                 obj.Im = obj.Ir+ obj.I1/(obj.n^2); 
%                 obj.Bm = obj.Br+ obj.B1/(obj.n^2);
%             b1_factor = repmat(obj.B1/((obj.n.*obj.n_factor).^2),[obj.n_group 1]);
%             i1_factor = repmat(obj.I1/((obj.n.*obj.n_factor).^2),[obj.n_group 1]);
            
            b1_factor = obj.B1./(obj.n_factor.^2);
            i1_factor = obj.I1./(obj.n_factor.^2);

            ddot_theta = zeros(obj.n_group,1);
            
            %有问题
            %ddot_theta(:,1) = ((obj.m*obj.g*obj.l*obj.is_g_enable*sin(obj.state(:,1)./(obj.n*obj.n_factor)))./(obj.n*obj.n_factor)+input_tau-((obj.Bm+obj.is_g_enable*b1_factor).*obj.state(:,2)))./(obj.Im+obj.is_g_enable*i1_factor);
            
            tau_g = obj.m* obj.g * obj.l *obj.is_g_enable* sin(obj.joint_state(:,1));
            
            ddot_theta(:,1) = (input_tau - ...
              ((obj.Bm+obj.is_g_enable*b1_factor).*obj.state(:,2)) +...
              tau_g./obj.n_factor...
            )...
                ./(obj.Im+obj.is_g_enable*i1_factor);
            
            
            obj.state(:,3) = ddot_theta;
            obj.state(:,2) = obj.state(:,2)+ ddot_theta*simu_period;
            obj.state(:,1) = obj.state(:,1)+ obj.state(:,2)*simu_period;
            %这些都是转子部分的
            
            
            %现在才处理关节末端的量
            obj.joint_state(:,3) = ddot_theta./obj.n_factor;%没有太多参考意义
            obj.joint_state(:,2) = obj.state(:,2)./obj.n_factor;
            obj.joint_state(:,1) = obj.joint_state(:,1)+ obj.joint_state(:,2)*simu_period;
            %所以处理超限度是在外面
%             negative_rows = find(obj.state(:,1)< 0,obj.n_group);
%             tmp = obj.state./(obj.n*obj.n_factor);
%             tmp(negative_rows,1) = mod(obj.state(negative_rows,1),2*pi);
            % not using    obj.state(1) >2*pi ||
            %             disp(size(obj.state));
            state_info = obj.joint_state;
            
        end
        
        
        
        function state_info = pos_step_control(obj,kp,ki,kd,sample_period,simu_step,simu_period)
            %这个是位置阶跃相应的一系列操作
            target_theta = pi/2-1*pi/180;
            pos_pid = pidControler(obj.n_group,kp,ki,kd,target_theta);
            disp(pos_pid.ref_value);
            disp(pos_pid.kp);
            disp(pos_pid.ki);
            disp(pos_pid.kd);
       
            state_info = zeros(obj.n_group,length(1:simu_step),3);
            debug_info = zeros(obj.n_group,length(1:simu_step),4);
            %pos_pid.get_add_up_con_value(obj.state(1))
            con_value = 0;
            sample_time_count = 0;
            for i = 1:simu_step
                state_info(:,i,:) = (obj.simu_loop(con_value,simu_period)).';
                sample_time_count = sample_time_count+simu_period;
                if(sample_time_count >= sample_period)
%                     con_value = pos_pid.get_sum_up_con_value(obj.state(1));
                    debug_info(:,i,:) = pos_pid.get_sum_up_con_value(obj.state(:,1));
                    con_value = debug_info(:,i,1);
                    sample_time_count = sample_time_count-sample_period;
                end
                
            %debug看是不是类的问题，不是！是目标位置设置不对，就89°即可
%             ref_value = pi/2-1*pi/180;
%             simu_period = 1e-5;                        
%             sample_period = 1e-3;
% 
%             state_info = zeros(length(1:simu_step),3);
%             debug_info = zeros(length(1:simu_step),4);
%             con_value = 0;
%             sample_time_count = 0;
%             err_sum = 0;
%             err_last = 0;
%             err_now = 0;
%             for i = 1:simu_step
%                 
%                 state_info(i,:) = (obj.simu_loop(con_value,simu_period)).';
%                 sample_time_count = sample_time_count+simu_period;
%                 if(sample_time_count >= sample_period)
%                     err_now = ref_value - obj.state(1);
%                     if(err_now < -pi)
%                         err_now = mod(err_now,pi);
%                     elseif(err_now > pi)
%                         err_now = mod(err_now,-pi);
%                     end
%                     err_sum = err_sum + err_now;
%                     con_value = kp * err_now + ki * err_sum + kd * (err_now - err_last);
%                     debug_info(i,3) =  err_last;
%                     err_last = err_now;
%                     debug_info(i,1) = con_value;
%                     debug_info(i,2) = err_now;
%                     debug_info(i,4) = err_sum;
%                     sample_time_count = sample_time_count-sample_period;
%                 end
%                 
% 


            end                
        end
        
            
        function state_info = pos_traj_control(obj,kp,ki,kd,sample_period,simu_step,simu_period,target_traj)
            pos_pid = pidControler(kp,ki,kd,target_traj(1));
            state_info = zeros(length(1:simu_step),3);
            debug_info = zeros(length(1:simu_step),4);
            con_value = 0;
            sample_time_count = 0;
            for i = 1:simu_step
                state_info(i,:) = (obj.simu_loop(con_value,simu_period)).';
                sample_time_count = sample_time_count+simu_period;
                if(sample_time_count >= sample_period)
                    pos_pid.set_target(target_traj(1,i));
                    debug_info(i,:) = pos_pid.get_sum_up_con_value(obj.state(1));
                    con_value = debug_info(i,1);
                    sample_time_count = sample_time_count-sample_period;
                end
    
            end    
        end
        
        

        
        
        function disable_g(obj,zero)
            obj.is_g_enable = zero;  
            obj.Im = obj.Ir;
            obj.Bm = obj.Br;
        end
        
        
        
        function tau_info = inverse_dynamic(obj,desired_ddtheta)
            %返回参数为1，总期望力矩，2，干扰力矩，3，线性力矩
%             b1_factor = repmat(obj.B1/((obj.n.*obj.n_factor).^2),[obj.n_group 1]);
%             i1_factor = repmat(obj.I1/((obj.n.*obj.n_factor)^2),[obj.n_group 1]);
            b1_factor = obj.B1./(obj.n_factor.^2);
            i1_factor = obj.I1./(obj.n_factor.^2);

            tau_info = zeros(obj.n_group,3);                      

            interrupt_tau = obj.m*obj.g*obj.l*obj.is_g_enable*sin(obj.joint_state(:,1))./obj.n_factor;
            linear_tau  = desired_ddtheta.*(obj.Im+obj.is_g_enable*i1_factor) + ((obj.Bm+obj.is_g_enable*b1_factor).*obj.joint_state(:,2));
            
            
            
            desired_tau = interrupt_tau+linear_tau;
            
            tau_info(:,1) = desired_tau;
            tau_info(:,2) = interrupt_tau;
            tau_info(:,3) = linear_tau;    
            
        end
        
        function update_m_l(obj,err)
            obj.m = err* obj.m;
            obj.l = err* obj.l;
        end
        
        
    end
    
    
end